#include "Control_LQG.h"

using namespace Eigen;
const double carrFreq = 2444.97e6;
const double ErrorThreshold_fact = 0.05;
const double PrangeStd = 0.1;   //伪距测距精度
const double DopplerStd = 0.05;   //多普勒精度
const double R_P = (PrangeStd/299792458.0*1e9)*(PrangeStd/299792458.0*1e9); //ns
const double R_D = (DopplerStd/carrFreq*1e9)*(DopplerStd/carrFreq*1e9);     //ppb

Control_LQG::Control_LQG(/* args */)
{
    cnt = 0;
    ctrl_interval = 5;
    State = 0;
    u = 0;
    delay = 3;
    dr = 0.5 + delay/(ctrl_interval*2);
    Estimate_X.setZero();
    R.setZero();
    R(0, 0) = R_P; R(1, 1) = R_D;
    P_old.setIdentity();
    Qw.setZero();
    Qw(1,1) = 1e-8;
    F.setIdentity();
    I.setIdentity();
    H.setIdentity();
    H(0) = 1;
    Fdr.setIdentity();
    B(1) = 1;
    G(0) = 0.0795; G(1) = 0.3687;
    L_sum.setZero();
}

Control_LQG::~Control_LQG()
{
}

Output_X Control_LQG::Input_meas(const Input_X &meas)
{
    Output_X out;
    
    if(State==0){
        this->cnt++;
        this->LastTime = meas.Time;
        // 调整晶振频率
        // 频率偏差误差, 单位：ppb
        out.fre_offset = meas.fre_offset_Doppler/carrFreq*1e9;
        out.valid = true;
        State = 1;
        return out;
    }
    else if(State==1){
        this->cnt++;
        this->LastTime = meas.Time;
        // 判断频率偏差是否足够小，太大就继续等待，足够小就进入下一阶段，调整PPS相位
        if(abs(meas.fre_offset_Doppler)>0.5){
            return out;
        }
        out.PPS_Offset = meas.Clock_offset_Prange - this->PPS_offset;
        this->PPS_offset = meas.Clock_offset_Prange;
        out.fre_offset = 0;
        out.valid = true;
        State = 2;
        this->cnt = 0;
        return out;
        
    }
    else{
        this->cnt++;
        Vector2d L;
        L(0) = meas.Clock_offset_Prange - this->PPS_offset;
        L(1) = meas.fre_offset_Doppler/carrFreq;
        if(abs(L(0))>500){
            double dt = meas.Time - this->LastTime;
            this->cnt++;
            this->LastTime = meas.Time;
            if(dt>100){
                // 重新初始化
                State = 0;
            }
            return out;
        }
        L_sum+=L;
        if(this->cnt%ctrl_interval ==0){
            L = L_sum/ctrl_interval;
            double dt = meas.Time - this->LastTime;
            this->F(0,1) = dt;
            this->B(0) = dt*(1-this->dr);
            this->Fdr(0, 1) = dt*this->dr;
            // 时间更新
            MatrixXd pre_X = (F-B*G.transpose()*Fdr)*Estimate_X;
            MatrixXd P_ = F*P_old*F.transpose() + dt*dt*Qw;
            // 量测更新
            MatrixXd K = P_*H.transpose()*(H*P_*H.transpose() +R).inverse();
            Estimate_X = pre_X + K*(L - H*pre_X);
            P_old = (I-K*H)*P_;
            // control
            this->u = -this->G.transpose()*Estimate_X;
            out.fre_offset = u;
            out.valid = true;
            return out;
        }
        else
        {
            return out;
        }
        
    }
 
}
